﻿using System;
using System.Collections.Generic;

namespace T68Control.S12L_M1
{
    internal class KDTree
    {
        private class Node
        {
            public (double x, double y) Point { get; set; }
            public Node Left { get; set; }
            public Node Right { get; set; }
        }

        private Node root;

        public KDTree(List<(double x, double y)> points)
        {
            root = BuildKDTree(points, depth: 0);
        }

        private Node BuildKDTree(List<(double x, double y)> points, int depth)
        {
            if (points.Count == 0)
                return null;

            int axis = depth % 2;
            points.Sort((a, b) => axis == 0 ? a.x.CompareTo(b.x) : a.y.CompareTo(b.y));
            int median = points.Count / 2;

            var node = new Node
            {
                Point = points[median],
                Left = BuildKDTree(points.GetRange(0, median), depth + 1),
                Right = BuildKDTree(points.GetRange(median + 1, points.Count - median - 1), depth + 1)
            };

            return node;
        }

        public (double x, double y) FindNearestNeighbor((double x, double y) target)
        {
            return FindNearest(root, target, depth: 0).Point;
        }

        private Node FindNearest(Node node, (double x, double y) target, int depth)
        {
            if (node == null)
                return null;

            int axis = depth % 2;
            Node nextBranch = null, oppositeBranch = null;

            if ((axis == 0 && target.x < node.Point.x) || (axis == 1 && target.y < node.Point.y))
            {
                nextBranch = node.Left;
                oppositeBranch = node.Right;
            }
            else
            {
                nextBranch = node.Right;
                oppositeBranch = node.Left;
            }

            Node best = CloserDistance(target, FindNearest(nextBranch, target, depth + 1), node);

            if (oppositeBranch != null)
            {
                if ((axis == 0 && Math.Abs(target.x - node.Point.x) < CalculateDistance(target, best.Point)) ||
                    (axis == 1 && Math.Abs(target.y - node.Point.y) < CalculateDistance(target, best.Point)))
                {
                    best = CloserDistance(target, FindNearest(oppositeBranch, target, depth + 1), best);
                }
            }

            return best;
        }

        private Node CloserDistance((double x, double y) target, Node a, Node b)
        {
            if (a == null) return b;
            if (b == null) return a;

            double distanceA = CalculateDistance(target, a.Point);
            double distanceB = CalculateDistance(target, b.Point);

            return distanceA < distanceB ? a : b;
        }

        private double CalculateDistance((double x, double y) point1, (double x, double y) point2)
        {
            return Math.Sqrt(Math.Pow(point1.x - point2.x, 2) + Math.Pow(point1.y - point2.y, 2));
        }
    }
}